#include "vsPhotometric.h"


extern RosConnector *connector;



VS_4Point::VS_4Point(std::vector<cv::Point2f> des_Four_Centers_, float focalLengthPIX_, float u0_, float v0_,std::vector<double> des_apriltag_positions_)
{
	focalLengthPIX = focalLengthPIX_;
	u0 = u0_;
	v0 = v0_;


	desiredImageFeature_pix << des_Four_Centers_.at(0).x - u0, des_Four_Centers_.at(0).y - v0,  //期望点坐标 单位：in pixel
		des_Four_Centers_.at(1).x - u0, des_Four_Centers_.at(1).y - v0,
		des_Four_Centers_.at(2).x - u0, des_Four_Centers_.at(2).y - v0,
		des_Four_Centers_.at(3).x - u0, des_Four_Centers_.at(3).y - v0;

	// std::cout<<"desiredImageFeature_pix:  "<<desiredImageFeature_pix << std::endl;


  des_apriltag_positions=des_apriltag_positions_;

#ifdef ESTIMATED_DEPTH
	// 设置四个图像点的深度值相同，且均为Z
	Z1 = ZInCam;
	Z2 = ZInCam;
	Z3 = ZInCam;
	Z4 = ZInCam;
#endif // ESTIMATED_DEPTH


	//期望点坐标 in pixel
	u_d << desiredImageFeature_pix(0, 0), desiredImageFeature_pix(2, 0), desiredImageFeature_pix(4, 0), desiredImageFeature_pix(6, 0);
	v_d << desiredImageFeature_pix(1, 0), desiredImageFeature_pix(3, 0), desiredImageFeature_pix(5, 0), desiredImageFeature_pix(7, 0);



}

void VS_4Point::servoing()
{
	
#ifdef REAL_DEPTH
		Z1 = 0.03;
		Z2 = 0.03;
		Z3 = 0.03;
		Z4 = 0.03;
		

#endif // REAL_DEPTH
		servoingLaw();

	
}



// 基于特征点的视觉伺服控制律
void VS_4Point::servoingLaw()
{


   //当前apriltag中心点三维坐标
   current_center_positions << connector->apriltag_position_posture[0], connector->apriltag_position_posture[1], connector->apriltag_position_posture[2];
//    vector<double> delta_pos;

    
   for(int i=0;i<3;++i)   //位置差 在相机坐标系内的 单位： m
   {
	delta_pos(i,0)=des_apriltag_positions[i]-current_center_positions[i];
   }
    //从这加对B1等螺钉对Z轴方向的单独处理
	if((connector->i_operate == 1 && connector->i_screw == 0)
	||(connector->i_operate == 2 && connector->i_screw == 0)
)
	{
		std::cout<<"修改期望位置Z方向"<<std::endl;
		delta_pos(2,0)=delta_pos(2,0)+0.005; //设置Z方向期望位置对偏移量//单位是cm
	}

	if((connector->i_operate == 6 && connector->i_screw == 0)
	||(connector->i_operate == 7 && connector->i_screw == 0))
	{
				std::cout<<"修改期望位置Z方向"<<std::endl;
		delta_pos(2,0)=delta_pos(2,0)+0.01; //设置Z方向期望位置对偏移量//单位是cm
	}
	std::cout<< "des_apriltag_positions:"<<des_apriltag_positions[0] <<" "<<des_apriltag_positions[1]<<" "<<des_apriltag_positions[2]<<std::endl;
	std::cout<< "current_center_positions:"<<current_center_positions[0] <<" "<<current_center_positions[1]<<" "<<current_center_positions[2]<<std::endl;

//直接用这个位置差控制电机转
    MatrixXd R7C(3,3);
	// T7C << 0, -cos(20*pi/180), -sin(20*pi/180), -0.063945, 0, -sin(20*pi/180), cos(20*pi/180), -0.028714, -1, 0, 0, 0.0004, 0, 0, 0, 1;  //仿真
	R7C << 0, -cos(20*pi/180), -sin(20*pi/180), 0, -sin(20*pi/180), cos(20*pi/180), -1, 0, 0;  //实机


    // MatrixXd delta_pos_in_end(3,1);
    delta_pos_in_end=R7C*delta_pos;	
 
	projectPoint_pix << connector->apriltag_corpoint[0] - u0, connector->apriltag_corpoint[1] - v0,
		connector->apriltag_corpoint[2] - u0, connector->apriltag_corpoint[3] - v0,
		connector->apriltag_corpoint[4] - u0, connector->apriltag_corpoint[5] - v0,
		connector->apriltag_corpoint[6] - u0, connector->apriltag_corpoint[7] - v0;	

	//当前期望点坐标（u，v）in pixel
	u_current << projectPoint_pix(0, 0), projectPoint_pix(2, 0), projectPoint_pix(4, 0), projectPoint_pix(6, 0);
	v_current << projectPoint_pix(1, 0), projectPoint_pix(3, 0), projectPoint_pix(5, 0), projectPoint_pix(7, 0);

	//特征点坐标误差 in pixels
	e_u = u_current - u_d;
	e_v = v_current - v_d;
	e_u_v << e_u(0), e_v(0), e_u(1), e_v(1), e_u(2), e_v(2), e_u(3), e_v(3); //用于计算


	// std::cout<<"e_u_v: "<<e_u_v<<std::endl;


	//交互矩阵
	interactionMatrix_inPixel <<
		-focalLengthPIX / Z1, 0, u_current(0) / Z1, (u_current(0) * v_current(0)) / focalLengthPIX, -(focalLengthPIX * focalLengthPIX + u_current(0) * u_current(0)) / focalLengthPIX, v_current(0),
		0, -focalLengthPIX / Z1, v_current(0) / Z1, (focalLengthPIX * focalLengthPIX + v_current(0) * v_current(0)) / focalLengthPIX, -(u_current(0) * v_current(0)) / focalLengthPIX, -u_current(0),
		-focalLengthPIX / Z2, 0, u_current(1) / Z2, (u_current(1) * v_current(1)) / focalLengthPIX, -(focalLengthPIX * focalLengthPIX + u_current(1) * u_current(1)) / focalLengthPIX, v_current(1),
		0, -focalLengthPIX / Z2, v_current(1) / Z2, (focalLengthPIX * focalLengthPIX + v_current(1) * v_current(1)) / focalLengthPIX, -(u_current(1) * v_current(1)) / focalLengthPIX, -u_current(1),
		-focalLengthPIX / Z3, 0, u_current(2) / Z3, (u_current(2) * v_current(2)) / focalLengthPIX, -(focalLengthPIX * focalLengthPIX + u_current(2) * u_current(2)) / focalLengthPIX, v_current(2),
		0, -focalLengthPIX / Z3, v_current(2) / Z3, (focalLengthPIX * focalLengthPIX + v_current(2) * v_current(2)) / focalLengthPIX, -(u_current(2) * v_current(2)) / focalLengthPIX, -u_current(2),
		-focalLengthPIX / Z4, 0, u_current(3) / Z4, (u_current(3) * v_current(3)) / focalLengthPIX, -(focalLengthPIX * focalLengthPIX + u_current(3) * u_current(3)) / focalLengthPIX, v_current(3),
		0, -focalLengthPIX / Z4, v_current(3) / Z4, (focalLengthPIX * focalLengthPIX + v_current(3) * v_current(3)) / focalLengthPIX, -(u_current(3) * v_current(3)) / focalLengthPIX, -u_current(3);



	////没有误差映射 等价于经典四点程序
	lamda = 0.08;
	cameraVel = -lamda * pinv(interactionMatrix_inPixel) * e_u_v;   //计算相机速度   

	//std::cout<<"e_u_v:"<<e_u_v<<std::endl;

}







void VS_4Point::VScontrol_simulation()
{

	// std::cout<<"-----------------------FLAG 开始VScontrol_simulation----------------------"<<endl;

    // ros::spinOnce();
    for(int i=0; i<6; i++){
        // connector->joint_simulation_position_rec[i] = connector->joint_simulation_position[i];
        connector->joint_simulation_position_rec[i] = connector->motors[i];
    }

	std::cout<<"motors:    "<<connector->joint_simulation_position_rec[0]<<"  "<<connector->joint_simulation_position_rec[1]<<"  "<<connector->joint_simulation_position_rec[2]<<"  "<<connector->joint_simulation_position_rec[3]<<"  "<<endl;

	int apriltag_corpoint_outnum = 0;
	// ofstream outFile; // 创建流对象
	// outFile.open("/home/lqx/桌面/visservo_test.txt", ios::app); // 打开文件

	std::vector<double> apriltag_corpoint_rec = {0,0,0,0,0,0,0,0};

	int loop_total = 6000;
	// int loop_total = 2;

	std::vector<double> pos_param = {0,0,0,0,0,0};

	std::ofstream outfile("/home/nuc/桌面/data.txt");
	int num_apritag_not_found=0;
	// outfile.open("data.txt", ios::binary | ios::app | ios::in | ios::out);
	for (int loop = 0; loop < loop_total; loop++)      /////////////////伺服   loop设置为20000
	{
		
		// 伺服控制
	    
		servoing();                          //速度控制



		if(connector->apriltag_position_posture[6] != 0){
			
			double delta_pos_max = delta_pos.maxCoeff();
			double delta_pos_min = delta_pos.minCoeff(); 
			MatrixXd delta_pos_control(4,1);

			if((std::abs(delta_pos_min) < 0.0015)&&(std::abs(delta_pos_max) < 0.0015))
			{
				delta_pos_control<<0,0,0;
				std::cout<<"success !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
				VS_success = true;
				break;
			}else if(connector->limited_joint_outsend != 0){
				if(loop < 5){
					std::cout<<"start_joint_limited_failed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
				}else{
					std::cout<<"running_joint_limited_failed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
				}
				connector->limited_joint_outsend = 0;
				break;
			}else if(loop == loop_total - 1){
				std::cout<<"time_out_failed !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
				break;
			}
			std::cout<<"delta_pos: "<<delta_pos(0,0)<<" "<<delta_pos(1,0)<<" "<<delta_pos(2,0)<<std::endl;
			// std::cout<<"delta_pos_in_end: "<<delta_pos_in_end(0,0)<<" "<<delta_pos_in_end(1,0)<<" "<<delta_pos_in_end(2,0)<<std::endl;
			

			//直接用相机坐标系进行控制
			delta_pos_control(0,0)= -0.005*(delta_pos(0,0)+(delta_pos(0,0)>0?1:-1)*0.005)/0.25175;  //1/d    //偏摆舵机 x方向 4
			delta_pos_control(1,0)= -0.005*(delta_pos(1,0)+(delta_pos(1,0)>0?1:-1)*0.005)/0.25175;   //fuyang舵机 y方向    // 5
			delta_pos_control(2,0)= -0.005*((delta_pos(2,0)+(delta_pos(2,0)>0?1:-1)*0.005)/(0.436+0.25175));           // 大舵机 z方向   /  1
			//delta_pos_control(3,0)= -0.005*((delta_pos(2,0)+(delta_pos(2,0)>0?1:-1)*0.005)/(0.436+0.25175))/5;     //2关节
			outfile << delta_pos(0,0) <<" "<< delta_pos(1,0) <<" "<< delta_pos(2,0) <<"\n";




			//手动迁移
		





			connector->kine.Param_joints_update_simulation(connector->motors);			
			if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7){
				pos_param = connector->kine.kec_joints2End_addh(connector->i_operate);
			}else{
				pos_param = connector->kine.kec_joints2End();
			}


	        //13 14号螺钉z方向误差使用手动左右运动控制   10号
			if((connector->i_operate == 4 && connector->i_screw == 3)
			||(connector->i_operate == 7 && connector->i_screw == 2))

			{
				pos_param[2] = pos_param[2] + 2*3*100*delta_pos_control(2,0) ;//  //扩大三倍
				std::cout<<"pos_param[2]: "<<pos_param[2] <<std::endl;
			}else if((connector->i_operate == 9 && connector->i_screw == 1)
			||(connector->i_operate == 2 && connector->i_screw == 2))
			{
				pos_param[2] = pos_param[2] - 2*3*100*delta_pos_control(2,0) ;//往左为正号  //扩大三倍  //
			}else
			{
			    pos_param[0] = pos_param[0] + 3*100*delta_pos_control(2,0) ;//x方向增量控制 加号后面填前后运动，前为正  //扩大三倍
			}



			connector->kine.Param_writeto_Inv_sol(pos_param);
			std::vector<double> motors_res;
			// if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7){
			// 	motors_res=connector->kine.Inv_sol_addh(connector->i_operate);
			// }else{
				motors_res=connector->kine.Inv_sol(connector->i_operate, connector->i_screw );

				connector->kine.Param_joints_update_simulation_for(motors_res);

			// }
			connector->motors[0] = motors_res[0];
			connector->motors[1] = motors_res[1];
			connector->motors[2] = motors_res[2];
			connector->motors[6] = motors_res[6];
			connector->motors[7] = motors_res[7];
			connector->motors[8] = motors_res[8];
			connector->motors[9] = motors_res[9];
			connector->motors[10] = motors_res[10];
            
			std::cout<<"自动寻位关节1:  "<<connector->motors[0]<<endl;

			std::cout<<"自动寻位升降高度:  "<<connector->motors[10]<<endl;
			//std::cout<<"末端执行器位姿"<<pos_param[0]<<" "<<pos_param[1]<<" "<<pos_param[2]<<" "<<pos_param[3]<<" "<<pos_param[4]<<" "<<pos_param[5]<<std::endl;

			//直接用相机坐标系进行控制
			//connector->motors[0]=connector->motors[0]+delta_pos_control(2,0);
			// connector->motors[0]=connector->motors[0]+0;     //大舵机 向上抬为正   舵机逆时针为正
			connector->motors[3]=connector->motors[3]+delta_pos_control(0,0);  //左右偏摆舵机 往右为正   ///
			connector->motors[4]=connector->motors[4]-delta_pos_control(1,0);  //上下fuyang舵机 往上为正  //
			//connector->motors[1]=connector->motors[1]+delta_pos_control(3,0);


			//std::cout<<"关节角度： "<<connector->motors[0]<<"  "<<connector->motors[3]<<"  "<<connector->motors[4]<<"  "<<endl;
			connector->pub_all_robot(connector->motors);
			num_apritag_not_found=0;
		}else{
			std::cout<<"apriltag未检测到!"<<endl;
			++num_apritag_not_found;
			if(num_apritag_not_found==400)
			{
				break;
			}
		}


		usleep(8000);
	}
	outfile.close();

	// outFile.close(); // 关闭文件

	 cout<<"------------------------退出视觉伺服程序-----------------"<<endl;
}


// 求矩阵的违逆
MatXf pinv(MatXf x)
{
	Eigen::JacobiSVD<MatXf> svd(x, Eigen::ComputeFullU | Eigen::ComputeFullV);
	float  pinvtoler = 1.e-8;
	MatXf singularValues_inv = svd.singularValues();
	for (long i = 0; i < x.cols(); ++i) {
		if (singularValues_inv(i) > pinvtoler)
			singularValues_inv(i) = 1.0 / singularValues_inv(i);
		else singularValues_inv(i) = 0;
	}
	return svd.matrixV()*singularValues_inv.asDiagonal()*svd.matrixU().transpose();
}